/********** Library Includes **************************************************/

#include <opencv/cv.h>

/********** Local Includes ****************************************************/

#include "plan.h"

#include "celldecompastar.h"

/********** Namespaces ********************************************************/

using namespace cv;
using namespace std;

/********** Public Methods ****************************************************/

Plan::Plan(const Mat &grid) :
	m_grid(grid),
	m_astar(new CellDecompAStar(grid)),
	m_step(0)
{
	
}

void Plan::buildPlan(const Point &start, const Point &goal, int blockSize) {
	m_start = Point(start.x / blockSize, start.y / blockSize);
	m_goal = Point(goal.x / blockSize, goal.y / blockSize);

	m_path = m_astar->plan(m_start, m_goal);
	m_step = 0;
}

bool Plan::done() {
	return m_step >= m_path.size();
}

cv::Point Plan::target() {
	return m_path[m_step];
}

void Plan::advance() {
	m_step++;
}

std::vector<cv::Point> Plan::plan() {
	return m_path;
}

int Plan::step() {
	return m_step;
}

Point Plan::goal() {
	return m_goal;
}